#ifndef GCCUTILS_H
#define GCCUTILS_H

#include <avr/io.h>

// define data in FLASH space upper 64kb
// makefile must contains -> LDFLAGS += -Wl,--section-start=.textupper=1xxxx
#if defined(RAMPZ)
  #define PROGMEM_UPPER __attribute__ ((section (".textupper")))
  #define SET_RAMPZ(value)  {RAMPZ = value;}

  #define pgm_read_byte_inc(addr) \
  (__extension__({                \
      uint8_t __result;           \
      __asm__                     \
      (                           \
          "elpm %0, Z+" "\n\t"    \
          : "=r" (__result),      \
            "=z" (addr)           \
          : "1" (addr)            \
      );                          \
      __result;                   \
  }))

  #define pgm_read_word_inc(addr) \
  (__extension__({                \
      uint16_t __result;          \
      __asm__                     \
      (                           \
          "elpm %A0, Z+" "\n\t"   \
          "elpm %B0, Z+" "\n\t"   \
          : "=r" (__result),      \
            "=z" (addr)           \
          : "1" (addr)            \
      );                          \
      __result;                   \
  }))

#else
  #define PROGMEM_UPPER PROGMEM
  #define SET_RAMPZ(value)

  #define pgm_read_byte_inc(addr) \
  (__extension__({                \
      uint8_t __result;           \
      __asm__                     \
      (                           \
          "lpm %0, Z+" "\n\t"     \
          : "=r" (__result),      \
            "=z" (addr)           \
          : "1" (addr)            \
      );                          \
      __result;                   \
  }))

  #define pgm_read_word_inc(addr) \
  (__extension__({                \
      uint16_t __result;          \
      __asm__                     \
      (                           \
          "lpm %A0, Z+" "\n\t"    \
          "lpm %B0, Z+" "\n\t"    \
          : "=r" (__result),      \
            "=z" (addr)           \
          : "1" (addr)            \
      );                          \
      __result;                   \
  }))

#endif

typedef union {
  uint16_t word;
  struct {
    uint8_t a,b;
  };
} uint16_t_u;

typedef union {
  uint32_t dword;
  struct {
    uint8_t a,b,c,d;
  };
} uint32_t_u;

typedef union {
  uint32_t dword;
  struct {
    uint16_t a,b;
  };
} uint32_t_s;

#define L8(v)    (((uint16_t_u)v).a)
#define H8(v)    (((uint16_t_u)v).b)

#define L16(v)   (((uint32_t_s)v).a)
#define H16(v)   (((uint32_t_s)v).b)

#define LL8(v)   (((uint32_t_u)v).a)
#define LH8(v)   (((uint32_t_u)v).b)
#define HL8(v)   (((uint32_t_u)v).c)
#define HH8(v)   (((uint32_t_u)v).d)

#define ROL(value)                       \
  asm volatile (                         \
		"rol %0"                "\n\t"   \
		"adc %0, __zero_reg__"  "\n\t"   \
		: "=r" (value)                   \
        : "0" (value));

// convert value to decimal string into buffer
// value >= 0 and <= 0xFFFF (65535)
// returns pointer to null terminator in buffer
#define UINT16_TO_DECIMAL(value, buffer) \
(__extension__({                            \
   uint8_t* __buf = buffer;                 \
   uint8_t __temp;                          \
   asm volatile(                            \
		"ldi   %2, lo8(0x2F)"       "\n\t"  \
".%=1:"                             "\n\t"  \
        "inc   %2"                  "\n\t"  \
        "subi  %A0, lo8(10000)"     "\n\t"  \
        "sbci  %B0, hi8(10000)"     "\n\t"  \
        "brcc  .%=1"                "\n\t"  \
        "clt"                       "\n\t"  \
        "cpi   %2, lo8(0x30)"       "\n\t"  \
        "breq  .%=2"                "\n\t"  \
        "set"                       "\n\t"  \
        "st    x+, %2"              "\n\t"  \
".%=2:"                             "\n\t"  \
		"ldi   %2, lo8(0x3A)"       "\n\t"  \
".%=3:"                             "\n\t"  \
        "dec   %2"                  "\n\t"  \
        "subi  %A0, lo8(-1000)"     "\n\t"  \
        "sbci  %B0, hi8(-1000)"     "\n\t"  \
        "brcs  .%=3"                "\n\t"  \
        "brts  .%=4"                "\n\t"  \
        "cpi   %2, lo8(0x30)"       "\n\t"  \
        "breq  .%=5"                "\n\t"  \
        "set"                       "\n\t"  \
".%=4:"                             "\n\t"  \
        "st    x+, %2"              "\n\t"  \
".%=5:"                             "\n\t"  \
        "ldi   %2, lo8(0x2F)"       "\n\t"  \
".%=6:"                             "\n\t"  \
        "inc   %2"                  "\n\t"  \
        "subi  %A0, lo8(100)"       "\n\t"  \
        "sbci  %B0, hi8(100)"       "\n\t"  \
        "brcc  .%=6"                "\n\t"  \
        "brts  .%=7"                "\n\t"  \
        "cpi   %2, lo8(0x30)"       "\n\t"  \
        "breq  .%=8"                "\n\t"  \
        "set"                       "\n\t"  \
".%=7:"                             "\n\t"  \
        "st    x+, %2"              "\n\t"  \
".%=8:"                             "\n\t"  \
        "ldi   %2, lo8(0x3A)"       "\n\t"  \
".%=9:"                             "\n\t"  \
        "dec   %2"                  "\n\t"  \
        "subi  %A0, lo8(-10)"       "\n\t"  \
        "brcs  .%=9"                "\n\t"  \
        "brts  .%=A"                "\n\t"  \
        "cpi   %2, lo8(0x30)"       "\n\t"  \
        "breq  .%=B"                "\n\t"  \
".%=A:"                             "\n\t"  \
        "st    x+, %2"              "\n\t"  \
".%=B:"                             "\n\t"  \
        "subi  %A0, lo8(-(0x30))"   "\n\t"  \
        "st    x+, %A0"             "\n\t"  \
        "st    x, r1"               "\n\t"  \
    	: "=r" (value),                     \
    	  "=x" (__buf),                     \
    	  "=a" (__temp)                     \
    	: "0" (value),                      \
    	  "x" (__buf)                       \
     );                                     \
     __buf;                                 \
}))

// uint8_t* uint16_to_decimal(uint16_t value, uint8_t* buffer) {return UINT16_TO_DECIMAL(value, buffer);}

#endif
